#include <QtGui>
#include "Spreadsheet.h"
#include "Helper.h"
#include "FileIO.h"
#include <vector>
#include <string>
#define _USE_MATH_DEFINES // this makes sure M_PI will be defined
#include <math.h>
#include "Matrix.h"
#include "Disturbance.h"
#include "PhiGamma.h"

CPhiGamma::CPhiGamma()
{
}

CPhiGamma::~CPhiGamma()
{
    clear();
}

void CPhiGamma::clear()
{
    SetControlValues().CleanUp();
    SetStateValues().CleanUp();
    SetOutputValues().CleanUp();
    //SetDistGains().clear();

    setPhi().CleanUp();            
    setGamma().CleanUp();
    setCee().CleanUp();
    setDee().CleanUp();

    setSpectraFileNames().clear();
    for(int i=0;i<(int)_vpDisturbances.size();i++)
    {
        CDisturbance* pDist = _vpDisturbances[i];
        if(pDist) delete pDist;
    }
    _vpDisturbances.clear();
}

void CPhiGamma::load(const vector<CSpreadsheet*> &vSpreadsheet_,const string& strFileName_ )
{
    clear();
    // load phigamma files
    {
        //load phigamma
        CSpreadsheet *pMtx = NULL; // Phi Gamma Cee Dee for velocity
        for(unsigned int i=0;i<vSpreadsheet_.size();i++)
        {
            if(vSpreadsheet_[i]->currentFile().contains(strFileName_.c_str()))
                pMtx = vSpreadsheet_[i];
        }
        // validate 
        if(!pMtx)
        {
            QString sError("Phigamma File : can not find any Phigamma file.\n \"");
            sError += pMtx->currentFile();
            sError += "\".\n";
            throw exception(sError.toStdString().c_str());
        }
        vector<double> vIn;
        pMtx->search("control_order",1,&vIn);            setControlOrder()=vIn[0];
        pMtx->search("num_states",1,&vIn);               SetNumStates()=vIn[0];
        pMtx->search("num_outputs",1,&vIn);              SetNumOutputs()=vIn[0];
        pMtx->search("num_controls",1,&vIn);             SetNumControls()=vIn[0];
        pMtx->search("num_disturbances",1,&vIn);         setNumDisturbances()=vIn[0];
        pMtx->search("# control_gain",1,&vIn);           SetControlGain()=vIn[0];
        
        int nNum = numDisturbances();
        vector<string> vsIn;
        pMtx->search("spectral_file_name",nNum,&vsIn);   setSpectraFileNames()=vsIn;
        
        CMatrix<double> mM;
        pMtx->searchMatrix("phi",&mM);                   setPhi()=mM;
        pMtx->searchMatrix("gamma",&mM);                 setGamma()=mM;
        pMtx->searchMatrix("cee",&mM);                   setCee()=mM;
        pMtx->searchMatrix("dee",&mM);                   setDee()=mM;

        _vpDisturbances.resize(numDisturbances());
        for(int i=0;i<numDisturbances();i++)
        {
            _vpDisturbances[i] = new CDisturbance();
            _vpDisturbances[i]->setNameDisturbance()=spectraFileNames()[i];
            _vpDisturbances[i]->load(vSpreadsheet_,spectraFileNames()[i]);
        }
    }
    return;
}

// the vector stores the absolute time in seconds for each frame. 
void CPhiGamma::precompute()
{
    if(_vpDisturbances.size()==0) 
    {
        throw exception("No disturbances were loaded.");
    }
    else if(_vpDisturbances.size()==1)
    {
        ;// nothing need to be check
    }
    else 
    {
        // check whether all disturbances are consistent.
        for(int i=1;i<(int)_vpDisturbances.size();i++)
        {
            if(_vpDisturbances[i]->trialLengthinSecs()!=_vpDisturbances[i-1]->trialLengthinSecs())
                throw exception("Trial length specified for each disturbances need to be same.");
            if(_vpDisturbances[i]->frameRate()!=_vpDisturbances[i-1]->frameRate())
                throw exception("Frame rate specified for each disturbances need to be same.");
            if(_vpDisturbances[i]->extraSecs()!=_vpDisturbances[i-1]->extraSecs())
                throw exception("Extra time specified for each disturbances should be same.");
            if(_vpDisturbances[i]->preRunTimeinSecs()!=_vpDisturbances[i-1]->preRunTimeinSecs())
                throw exception("Extra time specified for each disturbances should be same.");
        }
    }

    for(int i=0;i<(int)_vpDisturbances.size();i++)
    {
        CDisturbance* pDisturbance = _vpDisturbances[i];
        pDisturbance->precompute();
    }

    CDisturbance* pDisturbance = _vpDisturbances[0];
    int nTotalFrames = pDisturbance->totalFrames();
    // init variables
    SetControlValues().SetSize(nTotalFrames,1,0);
    SetStateValues()  .SetSize(nTotalFrames,GetNumStates(),0);
    SetOutputValues() .SetSize(nTotalFrames,GetNumOutputs(),0);
}
void CPhiGamma::precomputeEachTrial()
{
    for(int i=0;i<(int)_vpDisturbances.size();i++)
    {
        CDisturbance* pDisturbance = _vpDisturbances[i];
        pDisturbance->precomputeEachTrial();
    }
    return;
}
void CPhiGamma::UpdateOutput(int nCurrFrameIdx_,double dCurrControlState_)
{
    // indicates the row index into data matrices corresponding to the current frame. for first frame, this will be zero as it is initialized to -1 in Init()

    // to "understand" the following, see page 155 and 189 in Barb Sweet's thesis
    // allocate matrices
    CMatrix<double> mOut   (GetNumOutputs() ,1,0);  // current cumulative state (that will be our output at the end of the matrix calculations)
    CMatrix<double> mCumSC (GetNumStates() ,1,0);   // current cumulative state (that will be our output at the end of the matrix calculations)
    CMatrix<double> mCumSO (GetNumStates() ,1,0);   // old cumulative state (everything from beginning to now-1)
    CMatrix<double> mCurrCD(GetNumControls() + numDisturbances() ,1,0);  // current control and disturbance

    mCurrCD(0,0) = SetControlValues()(nCurrFrameIdx_,0) = GetControlGain() *(dCurrControlState_);
    for(int i=0;i<numDisturbances();i++)
        mCurrCD(i+1,0) = _vpDisturbances[i]->disturbanceValues()(nCurrFrameIdx_,0);

    // use NumControl data points offset to append disturbance "inputs" to matrix

    if (nCurrFrameIdx_ > 0) // we need to have previous data for this
    {
        CMatrix<double> mTemp = GetStateValues().GetRow(nCurrFrameIdx_-1).Transpose();
        mCumSO.Set(0,0,mTemp);
    }

    mCumSC = phi() *mCumSO + gamma() *mCurrCD; // see page 155 (D.2) in Barb Sweet's thesis
    mOut   = cee() *mCumSC + dee()   *mCurrCD; // see page 155 (D.1) in Barb Sweet's thesis

    // now store current states and outputs in data
    SetStateValues().SetRow(nCurrFrameIdx_,mCumSC.Transpose());
    SetOutputValues().SetRow(nCurrFrameIdx_,mOut.Transpose());
}

int CPhiGamma::frameRate()
{
    if(_vpDisturbances.empty()) throw exception("Frame rate is illegal, because no disturbances were loaded.");
    return _vpDisturbances[0]->frameRate();
}

void CPhiGamma::SaveToFile()
{
    return;
}




